/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*         Mike Phillips (put the planner in its own thread)
*********************************************************************/
#include <move_base/move_base.h>
#include <move_base_msgs/RecoveryStatus.h>
#include <cmath>

#include <boost/algorithm/string.hpp>
#include <boost/thread.hpp>

#include <geometry_msgs/Twist.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

namespace move_base {

  MoveBase::MoveBase(tf2_ros::Buffer& tf) :
    tf_(tf),
    as_(NULL),
    planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
    bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
    blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),
    recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),
    planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
    runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) {
       //todo 导航
    as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

    ros::NodeHandle private_nh("~");
    ros::NodeHandle nh;

    recovery_trigger_ = PLANNING_R;

    //get some parameters that will be global to the move base node
    std::string global_planner, local_planner;
    private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
    private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
    private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
    private_nh.param("global_costmap/global_frame", global_frame_, std::string("map"));
    private_nh.param("planner_frequency", planner_frequency_, 0.0);
    private_nh.param("controller_frequency", controller_frequency_, 20.0);
    private_nh.param("planner_patience", planner_patience_, 5.0);
    private_nh.param("controller_patience", controller_patience_, 15.0);
    private_nh.param("max_planning_retries", max_planning_retries_, -1);  // disabled by default

    private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
    private_nh.param("oscillation_distance", oscillation_distance_, 0.5);

    // parameters of make_plan service
    private_nh.param("make_plan_clear_costmap", make_plan_clear_costmap_, true);
    private_nh.param("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, true);

    //set up plan triple buffer
    planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
    latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
    controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();

    //set up the planner's thread
    planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));

      private_nh.param("pid_kp", kp, 0.5);
      private_nh.param("pid_kd", kd, 0.5);
      private_nh.param("pid_ki", ki, 0.1);
      private_nh.param("pid_time_out", time_out, 200);
      private_nh.param("pid_xy_goal_tolerance", xy_goal_tolerance, 0.1);
      private_nh.param("pid_yaw_goal_tolerance", yaw_goal_tolerance, 0.1);
      private_nh.param("pid_tolerate_xy_goal_tolerance", tolerate_xy_goal_tolerance, 0.05);
      private_nh.param("pid_tolerate_yaw_goal_tolerance", tolerate_yaw_goal_tolerance, 0.1);
      private_nh.param("pid_distance_threshold", distance_threshold, 0.35);
      private_nh.param("pid_isStartPid", isStartPid, true);
      private_nh.param("ispubobstacle", ispubobstacle, true);






      xy_goal_pid_pub = nh.advertise<std_msgs::String>("/getPidGoalToleranceMovebase", 1);
    //目标点精度规划的返回值 2022-08-17
   //   getPidGoalToleranceActionPub = nh.advertise<disinfect_msg::pid_goal_tolerance_action>("/getPidGoalToleranceAction", 10);
      //set up the planner's thread
      targetGoal_thread_ = new boost::thread(boost::bind(&MoveBase::targetGoal_thread, this));
     // m_ = new std::mutex;
    //  m_->lock();
     //------------------------------------------------------------------------------------------

    //for commanding the base
    vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
    current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );

    ros::NodeHandle action_nh("move_base");
    action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
    recovery_status_pub_= action_nh.advertise<move_base_msgs::RecoveryStatus>("recovery_status", 1);

    //we'll provide a mechanism for some people to send goals as PoseStamped messages over a topic
    //they won't get any useful information back about its status, but this is useful for tools
    //like nav_view and rviz
    ros::NodeHandle simple_nh("move_base_simple");
    goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));
 //todo sukai 清除全局地图,重新更新
    updateCleancostmap_sub_ = simple_nh.subscribe<std_msgs::String>("/updateCleancostmap", 1, &MoveBase::callback_updateCleancostmap, this);
         /*********这里是控制 到达角度的精度 true 开启精度调整，false 关闭 ***************/
      tolerance_trans_ratio_subscribe_ = nh.subscribe<std_msgs::Bool>("/isStartPid",1, boost::bind(&MoveBase::isStartPidfun, this, _1));

     cleanImuLichengji_pub_ = nh.advertise<std_msgs::String>("/cleanImuLichengji", 1);

     //todo接收关于发现障碍物的一个控制指令,与发布一个障碍物的继续指令 2022-12-026
      //todo sukai 2022-12-26
      resetpubobstacle_sub_ = simple_nh.subscribe<std_msgs::String>("/resetpubobstacle", 1, &MoveBase::callback_resetpubobstacle, this);

       //todo sukai 2022-12-26
      pubobstacle_pub_ = nh.advertise<std_msgs::String>("/pubobstacle", 1);

      /*********开启和关闭障碍物检测 true 开启，false 关闭 ***************/
      ispubobstacle_subscribe_ = nh.subscribe<std_msgs::Bool>("/ispubobstacle",1, boost::bind(&MoveBase::ispubobstaclefun, this, _1));  

	//定时任务
     timer2_  = simple_nh.createTimer(ros::Duration(3), &MoveBase::updatefun,this, false, true);//数据表数据更新后2秒刷新数据
 

    //we'll assume the radius of the robot to be consistent with what's specified for the costmaps
    private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
    private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
    private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
    private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);

    private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
    private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
    private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);

    // 创建全局路经规划器 create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
    
    
    /**
    这段代码是在创建一个名为"global_costmap"的costmap_2d::Costmap2DROS对象，
    其中tf_是一个tf::TransformListener对象。Costmap2DROS是一个ROS节点，
    它将costmap_2d::Costmap2D对象与ROS系统集成。它还提供了一些方便的功能，例如在ROS参数服务器上设置参数，以及在ROS中发布和订阅costmap。
    
    这个函数并没有直接订阅costmap数据。它创建了一个名为"global_costmap"的costmap_2d::Costmap2DROS对象，该对象将在ROS系统中发布和订阅costmap数据。在这个对象的构造函数中，它会调用costmap_2d::Costmap2DROS::start()函数，该函数会订阅ROS话题"/map"和"/map_updates"，并在ROS话题"/costmap_updates"上发布costmap数据。因此，这个函数创建的对象将订阅costmap数据。
    
    */
    planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);//全局规划地图
    planner_costmap_ros_->pause();//暂停全局代价地图的更新，但是不会关闭传感器话题的订阅

    //initialize the global planner  创建全局路经规划器初始化
    try {
      //pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_; //sukai 加载插件：bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
      planner_ = bgp_loader_.createInstance(global_planner);// 创建全局规划插件对象  global_planner= "navfn/NavfnROS"
      planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);//全局路经规划初始化
    } catch (const pluginlib::PluginlibException& ex) {
      ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
      exit(1);
    }

    //创建局部路经规划器 create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
    controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);//局部规划地图
    controller_costmap_ros_->pause();//暂停局部代价地图的更新，但是不会关闭传感器话题的订阅

    //create a local planner  创建局部路经规划器 初始化
    try {
      tc_ = blp_loader_.createInstance(local_planner);
      ROS_INFO("Created local_planner %s", local_planner.c_str());
      tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
    } catch (const pluginlib::PluginlibException& ex) {
      ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
      exit(1);
    }

    // Start actively updating costmaps based on sensor data
    //costmap_2d::Costmap2DROS::start()函数，该函数会订阅ROS话题"/map"和"/map_updates"，并在ROS话题"/costmap_updates"上发布costmap数据。因此，这个函数创建的对象将订阅costmap数据。
    //我们可以在ROS参数服务器上找到名为"global_costmap"的命名空间，其中包含"/map"话题的名称。我们可以使用以下命令在终端中查看ROS参数服务器上的参数：
    planner_costmap_ros_->start();//#订阅传感器话题，启动全局代价地图更新 
    controller_costmap_ros_->start();//#订阅传感器话题，启动局部代价地图更新

    //advertise a service for getting a plan
    make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);

    //advertise a service for clearing the costmaps
    clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);

    //if we shutdown our costmaps when we're deactivated... we'll do that now
    if(shutdown_costmaps_){
      ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");
      planner_costmap_ros_->stop();//#停止全局代价地图更新，并停止订阅传感器话题
      controller_costmap_ros_->stop();//#停止局部代价地图更新，并停止订阅传感器话题
    }

    //load any user specified recovery behaviors, and if that fails load the defaults
    if(!loadRecoveryBehaviors(private_nh)){
      loadDefaultRecoveryBehaviors();
    }

    //initially, we'll need to make a plan
    state_ = PLANNING;

    //we'll start executing recovery behaviors at the beginning of our list
    recovery_index_ = 0;

    //we're all set up now so we can start the action server
    as_->start();

    dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~"));
    dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2);
    dsrv_->setCallback(cb);
  }

		
				//定时执行
void MoveBase::updatefun(const ros::TimerEvent &time_event) {
		 try {
                       timer2_.stop();
			std_msgs::String data;
                       // cleanImuLichengji_pub_.publish(data);
		  ///  std::cout<<"============1=====定时执行=MoveBase::updatefun==================="<<std::endl;
			 }catch (std::exception e){
				std::cout<<  e.what()<<std::endl;
			    }
		}
		

    //todo sukai 
    void  MoveBase::callback_updateCleancostmap(std_msgs::String data){
        boost::shared_ptr<nav_core::RecoveryBehavior> cons_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));//恢复

        cons_clear->initialize("conservative_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);//planner_costmap_ros_:全局代价地图 ；controller_costmap_ros_：局部代价地图
        cons_clear->runBehavior();

        //苏凯 sukai planner_costmap_ros_ = costmap_2d::Costmap2DROS*  global_costmap | controller_costmap_ros_ =Costmap2DROS* local_costmap
        if(planner_costmap_ros_ != NULL){
            planner_costmap_ros_->updateMap();

        }
        if(controller_costmap_ros_ != NULL){
            controller_costmap_ros_->updateMap();

        }

    }

    //todo sukai 2022-12-26 障碍物相关
    void  MoveBase::callback_resetpubobstacle(std_msgs::String data){


    }

    /*********这里是控制 到达角度的精度 true 开启精度调整，false 关闭 ***************/
  void MoveBase::isStartPidfun(const std_msgs::BoolConstPtr &cbmsg) {
    isStartPid = cbmsg->data;
    ROS_WARN("这里是控制 到达角度的精度 true 开启精度调整，false 关闭");
  }
  
   /*********开启和关闭障碍物检测 true 开启，false 关闭 ***************/
  void MoveBase::ispubobstaclefun(const std_msgs::BoolConstPtr &cbmsg) {
    ispubobstacle = cbmsg->data;
    ROS_WARN("开启和关闭障碍物检测 true 开启，false 关闭");
  }

  void MoveBase::reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level){
    boost::recursive_mutex::scoped_lock l(configuration_mutex_);

    //The first time we're called, we just want to make sure we have the
    //original configuration
    if(!setup_)
    {
      last_config_ = config;
      default_config_ = config;
      setup_ = true;
      return;
    }

    if(config.restore_defaults) {
      config = default_config_;
      //if someone sets restore defaults on the parameter server, prevent looping
      config.restore_defaults = false;
    }

    if(planner_frequency_ != config.planner_frequency)
    {
      planner_frequency_ = config.planner_frequency;
      p_freq_change_ = true;
    }

    if(controller_frequency_ != config.controller_frequency)
    {
      controller_frequency_ = config.controller_frequency;
      c_freq_change_ = true;
    }

    planner_patience_ = config.planner_patience;
    controller_patience_ = config.controller_patience;
    max_planning_retries_ = config.max_planning_retries;
    conservative_reset_dist_ = config.conservative_reset_dist;

    recovery_behavior_enabled_ = config.recovery_behavior_enabled;
    clearing_rotation_allowed_ = config.clearing_rotation_allowed;
    shutdown_costmaps_ = config.shutdown_costmaps;

    oscillation_timeout_ = config.oscillation_timeout;
    oscillation_distance_ = config.oscillation_distance;


      try {
          kp = config.pid_kp;
          kd = config.pid_kd;
          ki = config.pid_ki;
          time_out = config.pid_time_out;
          xy_goal_tolerance = config.pid_xy_goal_tolerance;
          yaw_goal_tolerance = config.pid_yaw_goal_tolerance;
          tolerate_xy_goal_tolerance = config.pid_tolerate_xy_goal_tolerance;
          tolerate_yaw_goal_tolerance = config.pid_tolerate_yaw_goal_tolerance;
          distance_threshold = config.pid_distance_threshold;
          isStartPid = config.pid_isStartPid;
          ispubobstacle = config.ispubobstacle;
      }catch (std::exception e){

      }




    //----------------------------------------------


    if(config.base_global_planner != last_config_.base_global_planner) {
      boost::shared_ptr<nav_core::BaseGlobalPlanner> old_planner = planner_;
      //initialize the global planner 初始化全局规划器
      ROS_INFO("Loading global planner %s", config.base_global_planner.c_str());
      try {
        planner_ = bgp_loader_.createInstance(config.base_global_planner);

        // wait for the current planner to finish planning 等待当前计划员完成计划
        boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);

        // Clean up before initializing the new planner 在初始化新计划器之前进行清理
        planner_plan_->clear();
        latest_plan_->clear();
        controller_plan_->clear();
        resetState();
        planner_->initialize(bgp_loader_.getName(config.base_global_planner), planner_costmap_ros_);

        lock.unlock();
      } catch (const pluginlib::PluginlibException& ex) {
        ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \
                   containing library is built? Exception: %s", config.base_global_planner.c_str(), ex.what());
        planner_ = old_planner;
        config.base_global_planner = last_config_.base_global_planner;
      }
    }

    if(config.base_local_planner != last_config_.base_local_planner){
      boost::shared_ptr<nav_core::BaseLocalPlanner> old_planner = tc_;
      //create a local planner
      try {
        tc_ = blp_loader_.createInstance(config.base_local_planner);
        // Clean up before initializing the new planner  在初始化新计划器之前进行清理
        planner_plan_->clear();
        latest_plan_->clear();
        controller_plan_->clear();
        resetState();
        tc_->initialize(blp_loader_.getName(config.base_local_planner), &tf_, controller_costmap_ros_);
      } catch (const pluginlib::PluginlibException& ex) {
        ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \
                   containing library is built? Exception: %s", config.base_local_planner.c_str(), ex.what());
        tc_ = old_planner;
        config.base_local_planner = last_config_.base_local_planner;
      }
    }

    make_plan_clear_costmap_ = config.make_plan_clear_costmap;
    make_plan_add_unreachable_goal_ = config.make_plan_add_unreachable_goal;

    last_config_ = config;
  }

  void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){
    //在ROS目标回调中，将POSESTAMP封装在动作消息中，并重新发送到服务器
    ROS_DEBUG_NAMED("move_base","In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
    move_base_msgs::MoveBaseActionGoal action_goal;
    action_goal.header.stamp = ros::Time::now();
    action_goal.goal.target_pose = *goal;
     //sukai 清楚地图
    #if 1
   //  planner_costmap_ros_->resetLayers();

    controller_costmap_ros_->resetLayers();
    planner_costmap_ros_->resetLayers();//重置代价地图的所有图层
    ros::Duration(0.5).sleep();//
    #endif  


    action_goal_pub_.publish(action_goal);
  }

  void MoveBase::clearCostmapWindows(double size_x, double size_y){
    geometry_msgs::PoseStamped global_pose;

    //clear the planner's costmap
    getRobotPose(global_pose, planner_costmap_ros_);

    std::vector<geometry_msgs::Point> clear_poly;
    double x = global_pose.pose.position.x;
    double y = global_pose.pose.position.y;
    geometry_msgs::Point pt;

    pt.x = x - size_x / 2;
    pt.y = y - size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x + size_x / 2;
    pt.y = y - size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x + size_x / 2;
    pt.y = y + size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x - size_x / 2;
    pt.y = y + size_y / 2;
    clear_poly.push_back(pt);

    planner_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);

    //clear the controller's costmap
    getRobotPose(global_pose, controller_costmap_ros_);

    clear_poly.clear();
    x = global_pose.pose.position.x;
    y = global_pose.pose.position.y;

    pt.x = x - size_x / 2;
    pt.y = y - size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x + size_x / 2;
    pt.y = y - size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x + size_x / 2;
    pt.y = y + size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x - size_x / 2;
    pt.y = y + size_y / 2;
    clear_poly.push_back(pt);

    controller_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
  }

  bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp){
    //clear the costmaps
    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_controller(*(controller_costmap_ros_->getCostmap()->getMutex()));
    controller_costmap_ros_->resetLayers();

    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_planner(*(planner_costmap_ros_->getCostmap()->getMutex()));
    planner_costmap_ros_->resetLayers();
    return true;
  }


  bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp){
    if(as_->isActive()){
//move_base必须处于非活动状态才能为外部用户制定计划
      ROS_ERROR("move_base must be in an inactive state to make a plan for an external user");
      return false;
    }
    //make sure we have a costmap for our planner
    if(planner_costmap_ros_ == NULL){
      ROS_ERROR("move_base cannot make a plan for you because it doesn't have a costmap");
      return false;
    }

    geometry_msgs::PoseStamped start;
    //if the user does not specify a start pose, identified by an empty frame id, then use the robot's pose
    if(req.start.header.frame_id.empty())
    {
        geometry_msgs::PoseStamped global_pose;
        if(!getRobotPose(global_pose, planner_costmap_ros_)){
          ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot");
          return false;
        }
        start = global_pose;
    }
    else
    {
        start = req.start;
    }

    if (make_plan_clear_costmap_) {
      //update the copy of the costmap the planner uses
      clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_);
    }

    //first try to make a plan to the exact desired goal 首先尝试制定一个计划，以达到确切的预期目标
    std::vector<geometry_msgs::PoseStamped> global_plan;
    if(!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()){
      ROS_DEBUG_NAMED("move_base","Failed to find a plan to exact goal of (%.2f, %.2f), searching for a feasible goal within tolerance",
          req.goal.pose.position.x, req.goal.pose.position.y);

      //search outwards for a feasible goal within the specified tolerance 在规定的公差范围内向外寻找可行的目标
      geometry_msgs::PoseStamped p;
      p = req.goal;
      bool found_legal = false;
      float resolution = planner_costmap_ros_->getCostmap()->getResolution();
      float search_increment = resolution*3.0;
      if(req.tolerance > 0.0 && req.tolerance < search_increment) search_increment = req.tolerance;
      for(float max_offset = search_increment; max_offset <= req.tolerance && !found_legal; max_offset += search_increment) {
        for(float y_offset = 0; y_offset <= max_offset && !found_legal; y_offset += search_increment) {
          for(float x_offset = 0; x_offset <= max_offset && !found_legal; x_offset += search_increment) {

            //don't search again inside the current outer layer 不再在当前外层内部搜索
            if(x_offset < max_offset-1e-9 && y_offset < max_offset-1e-9) continue;

            //search to both sides of the desired goal 向目标两侧搜索
            for(float y_mult = -1.0; y_mult <= 1.0 + 1e-9 && !found_legal; y_mult += 2.0) {

              //if one of the offsets is 0, -1*0 is still 0 (so get rid of one of the two) 如果其中一个偏移是0，-1*0仍然是0（所以去掉两个偏移中的一个）
              if(y_offset < 1e-9 && y_mult < -1.0 + 1e-9) continue;

              for(float x_mult = -1.0; x_mult <= 1.0 + 1e-9 && !found_legal; x_mult += 2.0) {
                if(x_offset < 1e-9 && x_mult < -1.0 + 1e-9) continue;

                p.pose.position.y = req.goal.pose.position.y + y_offset * y_mult;
                p.pose.position.x = req.goal.pose.position.x + x_offset * x_mult;

                if(planner_->makePlan(start, p, global_plan)){
                  if(!global_plan.empty()){

                    if (make_plan_add_unreachable_goal_) {
                    //将（无法实现的）原始目标添加到全局计划的末尾，以防本地计划人员能够帮助您实现目标
                      //adding the (unreachable) original goal to the end of the global plan, in case the local planner can get you there
                      //(the reachable goal should have been added by the global planner)  （可达到的目标应该由全球规划师添加）
                      global_plan.push_back(req.goal);
                    }

                    found_legal = true;
                    ROS_DEBUG_NAMED("move_base", "Found a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
                    break;
                  }
                }
                else{
                  ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
                }
              }
            }
          }
        }
      }
    }

    //copy the plan into a message to send out
    resp.plan.poses.resize(global_plan.size());
    for(unsigned int i = 0; i < global_plan.size(); ++i){
      resp.plan.poses[i] = global_plan[i];
    }

    return true;
  }

  MoveBase::~MoveBase(){
    recovery_behaviors_.clear();

    delete dsrv_;

    if(as_ != NULL)
      delete as_;

    if(planner_costmap_ros_ != NULL)
      delete planner_costmap_ros_;

    if(controller_costmap_ros_ != NULL)
      delete controller_costmap_ros_;

    planner_thread_->interrupt();
    planner_thread_->join();

    delete planner_thread_;
     targetGoal_thread_->interrupt();
      targetGoal_thread_->join();
      delete targetGoal_thread_;
//      if(m_ != NULL)
//          delete m_;

      //-------------------------------------

    delete planner_plan_;
    delete latest_plan_;
    delete controller_plan_;

    planner_.reset();
    tc_.reset();
  }

  bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
    boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex()));

    //make sure to set the plan to be empty initially
    plan.clear();

    //since this gets called on handle activate
    if(planner_costmap_ros_ == NULL) {
      ROS_ERROR("Planner costmap ROS is NULL, unable to create global plan");
      return false;
    }

    //get the starting pose of the robot 得到机器人起始点
    geometry_msgs::PoseStamped global_pose;
    if(!getRobotPose(global_pose, planner_costmap_ros_)) {
      ROS_WARN("Unable to get starting pose of robot, unable to create global plan");
      return false;
    }

    const geometry_msgs::PoseStamped& start = global_pose;

    //if the planner fails or returns a zero length plan, planning failed 如果规划失败或者返回一个长度为0的规划，则规划失败
    if(!planner_->makePlan(start, goal, plan) || plan.empty()){
      ROS_DEBUG_NAMED("move_base","Failed to find a  plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y);
      return false;
    }

    return true;
  }

  void MoveBase::publishZeroVelocity(){
    geometry_msgs::Twist cmd_vel;
    cmd_vel.linear.x = 0.0;
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = 0.0;
    vel_pub_.publish(cmd_vel);
  }

  bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion& q){
    //first we need to check if the quaternion has nan's or infs
    if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){
      ROS_ERROR("Quaternion has nans or infs... discarding as a navigation goal");
      return false;
    }

    tf2::Quaternion tf_q(q.x, q.y, q.z, q.w);

    //next, we need to check if the length of the quaternion is close to zero
    if(tf_q.length2() < 1e-6){
      ROS_ERROR("Quaternion has length close to zero... discarding as navigation goal");
      return false;
    }

    //next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
    tf_q.normalize();

    tf2::Vector3 up(0, 0, 1);

    double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));

    if(fabs(dot - 1) > 1e-3){
      ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical.");
      return false;
    }

    return true;
  }
//获取目标点在全局坐标系下的坐标
  geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg){
    std::string global_frame = planner_costmap_ros_->getGlobalFrameID();
    geometry_msgs::PoseStamped goal_pose, global_pose;
    goal_pose = goal_pose_msg;

    //just get the latest available transform... for accuracy they should send
    //goals in the frame of the planner
    goal_pose.header.stamp = ros::Time();

    try{
      tf_.transform(goal_pose_msg, global_pose, global_frame);
    }
    catch(tf2::TransformException& ex){
      ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s",
          goal_pose.header.frame_id.c_str(), global_frame.c_str(), ex.what());
      return goal_pose_msg;
    }

    return global_pose;
  }

  void MoveBase::wakePlanner(const ros::TimerEvent& event)
  {
    // we have slept long enough for rate
    planner_cond_.notify_one();
  }
  //sukai 2022-08-31
    void MoveBase::targetGoal_thread(){
      ros::Rate rate(10);
        while(ros::ok()) {

            if ((abs(planner_goal_me_.pose.position.x) + abs(planner_goal_me_.pose.position.y) +
                 abs(planner_goal_me_.pose.orientation.z) + abs(planner_goal_me_.pose.orientation.w)) > 0.001) {

                tf::Transform transformBroadcaster;

                transformBroadcaster.setOrigin(
                        tf::Vector3{planner_goal_me_.pose.position.x, planner_goal_me_.pose.position.y, 0});

                tf::Quaternion quaternion;
                quaternion.setValue(0.0, 0.0, planner_goal_me_.pose.orientation.z, planner_goal_me_.pose.orientation.w);
                transformBroadcaster.setRotation(quaternion);
                broadcaster.sendTransform(
                        tf::StampedTransform(transformBroadcaster, ros::Time().now(), "/map", "/targetGoal"));

            }
            rate.sleep();

        }
    }





    //planner线程的入口。这个函数需要等待actionlib服务器的cbMoveBase::executeCb来唤醒启动。
 //主要作用是调用全局路径规划获取路径，同时保证规划的周期性以及规划超时清除goal
  void MoveBase::planThread(){
    ROS_DEBUG_NAMED("move_base_plan_thread","Starting planner thread...");
    ros::NodeHandle n;
    ros::Timer timer;
    bool wait_for_wake = false;
    boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
    while(n.ok()){

        //check if we should run the planner (the mutex is locked)
      while(wait_for_wake || !runPlanner_){
        //if we should not be running the planner then suspend this thread
        ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending");
        planner_cond_.wait(lock);
        wait_for_wake = false;
      }
      ros::Time start_time = ros::Time::now();

      //time to plan! get a copy of the goal and unlock the mutex
      geometry_msgs::PoseStamped temp_goal = planner_goal_;
      lock.unlock();
      ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");
//3. 获取规划的全局路径
         //这里的makePlan作用是获取机器人的位姿作为起点，然后调用全局规划器的makePlan返回规划路径，存储在planner_plan_
      //run planner
      planner_plan_->clear();
      bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
 //4.如果获得了plan,则将其赋值给latest_plan_
      if(gotPlan){
        ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size());
        //pointer swap the plans under mutex (the controller will pull from latest_plan_)
        std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_;

        lock.lock();
        planner_plan_ = latest_plan_;
        latest_plan_ = temp_plan;
        last_valid_plan_ = ros::Time::now();
        planning_retries_ = 0;
        new_global_plan_ = true;

        ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner");

        //make sure we only start the controller if we still haven't reached the goal
        if(runPlanner_)
          state_ = CONTROLLING;
        if(planner_frequency_ <= 0)
          runPlanner_ = false;
        lock.unlock();
      }
    //5. 达到一定条件后停止路径规划,进入清障模式
         //如果没有规划出路径，并且处于PLANNING状态，则判断是否超过最大规划周期或者规划次数
         //如果是则进入自转模式，否则应该会等待MoveBase::executeCycle的唤醒再次规划
      //if we didn't get a plan and we are in the planning state (the robot isn't moving)
      else if(state_==PLANNING){
//仅在MoveBase::executeCb及其调用的MoveBase::executeCycle或者重置状态时会被设置为PLANNING
             //一般是刚获得新目标，或者得到路径但计算不出下一步控制时重新进行路径规划
        ROS_DEBUG_NAMED("move_base_plan_thread","No Plan...");
        ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);

        //check if we've tried to make a plan for over our time limit or our maximum number of retries
        //issue #496: we stop planning when one of the conditions is true, but if max_planning_retries_
        //is negative (the default), it is just ignored and we have the same behavior as ever
        lock.lock();
        planning_retries_++;
        if(runPlanner_ &&
           (ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))){
          //we'll move into our obstacle clearing mode
          state_ = CLEARING;
          runPlanner_ = false;  // proper solution for issue #523
          publishZeroVelocity();
          recovery_trigger_ = PLANNING_R;
        }

        lock.unlock();
      }

      //take the mutex for the next iteration
      lock.lock();
       //6.设置睡眠模式
         //如果还没到规划周期则定时器睡眠，在定时器中断中通过planner_cond_唤醒，这里规划周期为0
      //setup sleep interface if needed
      if(planner_frequency_ > 0){
        ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now();
        if (sleep_time > ros::Duration(0.0)){
          wait_for_wake = true;
          timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
        }
      }
    }
  }








//  //sukai 2022-08-31
    bool MoveBase::isClosevariance(geometry_msgs::Pose &old_pose_,geometry_msgs::Pose &new_pose ,double distanceAccuracy,double angleAccuracy){
        bool robot_stopped_ = (tf2::Vector3(old_pose_.position.x, old_pose_.position.y,
                                            old_pose_.position.z).distance(tf2::Vector3(new_pose.position.x,
                                                                                        new_pose.position.y, new_pose.position.z)) < distanceAccuracy) &&
                              (tf2::Quaternion(old_pose_.orientation.x,
                                               old_pose_.orientation.y,
                                               old_pose_.orientation.z,
                                               old_pose_.orientation.w).angle(tf2::Quaternion(new_pose.orientation.x,
                                                                                              new_pose.orientation.y,
                                                                                              new_pose.orientation.z,
                                                                                              new_pose.orientation.w)) < angleAccuracy);

        std::string  current_goal_tolerance_pid =std::to_string(tf2::Vector3(old_pose_.position.x, old_pose_.position.y,
                                                                   old_pose_.position.z).distance(tf2::Vector3(new_pose.position.x,
                                                                                                               new_pose.position.y, new_pose.position.z)));
        std::string  current_yaw_goal_tolerance =std::to_string(tf2::Quaternion(old_pose_.orientation.x,
                                                                      old_pose_.orientation.y,
                                                                      old_pose_.orientation.z,
                                                                      old_pose_.orientation.w).angle(tf2::Quaternion(new_pose.orientation.x,
                                                                                                                     new_pose.orientation.y,
                                                                                                                     new_pose.orientation.z,
                                                                                                                     new_pose.orientation.w)));

        std_msgs::String datastr;
        datastr.data= "ActionisClosevariance";
        xy_goal_pid_pub.publish(datastr);
        return robot_stopped_;
    }


    //sukai 2022-08-31
    bool MoveBase::isClosevariance_distanceAccuracy(geometry_msgs::Pose &old_pose_,geometry_msgs::Pose &new_pose ,double distanceAccuracy){
        bool robot_stopped_ = (tf2::Vector3(old_pose_.position.x, old_pose_.position.y,
                                            old_pose_.position.z).distance(tf2::Vector3(new_pose.position.x,
                                                                                        new_pose.position.y, new_pose.position.z)) < distanceAccuracy);
        std::string  current_goal_tolerance_pid =std::to_string(tf2::Vector3(old_pose_.position.x, old_pose_.position.y,
                                                                   old_pose_.position.z).distance(tf2::Vector3(new_pose.position.x,
                                                                                                               new_pose.position.y, new_pose.position.z)));



        std_msgs::String datastr;
        datastr.data= "isClosevariance_distanceAccuracy";
        xy_goal_pid_pub.publish(datastr);
        return robot_stopped_;
    }
    //sukai 2022-08-31
    bool MoveBase::isClosevariance_angleAccuracy(geometry_msgs::Pose &old_pose_,geometry_msgs::Pose &new_pose, double angleAccuracy){
        bool robot_stopped_ =(tf2::Quaternion(old_pose_.orientation.x,
                                              old_pose_.orientation.y,
                                              old_pose_.orientation.z,
                                              old_pose_.orientation.w).angle(tf2::Quaternion(new_pose.orientation.x,
                                                                                             new_pose.orientation.y,
                                                                                             new_pose.orientation.z,
                                                                                             new_pose.orientation.w)) < angleAccuracy);

        std::string  current_yaw_goal_tolerance =std::to_string(tf2::Quaternion(old_pose_.orientation.x,
                                                                      old_pose_.orientation.y,
                                                                      old_pose_.orientation.z,
                                                                      old_pose_.orientation.w).angle(tf2::Quaternion(new_pose.orientation.x,
                                                                                                                     new_pose.orientation.y,
                                                                                                                     new_pose.orientation.z,
                                                                                                                     new_pose.orientation.w)));


        std_msgs::String datastr;
        datastr.data= "isClosevariance_angleAccuracy";
        xy_goal_pid_pub.publish(datastr);
        return robot_stopped_;
    }







    //sukai 2022-08-31
    void MoveBase::init(){

        goal_tolerance_pid =-1;
        distance_moveold=0.0;
        distanceold=0.0;

        curSpeed = 0;

        lastError = 0;

        totalError = 0;

        t_start =0;

        geometry_msgs::PoseStamped goalempty;
        planner_goal_me_=goalempty;

        geometry_msgs::Twist twist;
        vel_pub_.publish(twist);

  }



    //todo 导航
  void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
  {


    if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){
      as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
      return;
    }




    geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);

    publishZeroVelocity();
    boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);

    planner_goal_ = goal;
    planner_goal_me_ = goal;

    runPlanner_ = true;
    planner_cond_.notify_one();
    lock.unlock();

    current_goal_pub_.publish(goal);
    std::vector<geometry_msgs::PoseStamped> global_plan;

    ros::Rate r(controller_frequency_);
    if(shutdown_costmaps_){
      ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously");
      planner_costmap_ros_->start();
      controller_costmap_ros_->start();
    }

    //we want to make sure that we reset the last time we had a valid plan and control
    last_valid_control_ = ros::Time::now();
    last_valid_plan_ = ros::Time::now();
    last_oscillation_reset_ = ros::Time::now();
    planning_retries_ = 0;
//---------------------------------------------------------------------------------------------------------------------------------------------
  std::string message="";
    ros::NodeHandle n;
    while(n.ok())
    {
      if(c_freq_change_)
      {
        ROS_INFO("Setting controller frequency to %.2f", controller_frequency_);
        r = ros::Rate(controller_frequency_);
        c_freq_change_ = false;
      }

      if(as_->isPreemptRequested()){
        if(as_->isNewGoalAvailable()){

          move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
          if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){
            as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
            return;
          }


             goal = goalToGlobalFrame(new_goal.target_pose);

          //we'll make sure that we reset our state for the next execution cycle
          recovery_index_ = 0;
          state_ = PLANNING;

          lock.lock();
          planner_goal_ = goal;
          planner_goal_me_ = goal;
          runPlanner_ = true;
          planner_cond_.notify_one();
          lock.unlock();
          //publish the goal point to the visualizer
          ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
          current_goal_pub_.publish(goal);
          //make sure to reset our timeouts and counters
          last_valid_control_ = ros::Time::now();
          last_valid_plan_ = ros::Time::now();
          last_oscillation_reset_ = ros::Time::now();
          planning_retries_ = 0;
        }
        else {
          //if we've been preempted explicitly we need to shut things down
          resetState();
          //notify the ActionServer that we've successfully preempted
          ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");
          as_->setPreempted();//TODO 取消命令 sukai

           //we'll actually return from execute after preempting
          return;
        }
      }

      //we also want to check if we've changed global frames because we need to transform our goal pose
      if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){
        goal = goalToGlobalFrame(goal);


        //we want to go back to the planning state for the next execution cycle
        recovery_index_ = 0;
        state_ = PLANNING;

        //we have a new goal so make sure the planner is awake
        lock.lock();
        planner_goal_ = goal;
        planner_goal_me_ = goal;
        runPlanner_ = true;
        planner_cond_.notify_one();
        lock.unlock();

        //publish the goal point to the visualizer将目标点发布到可视化工具 , move_base的全局框架已更改，新fram
        ROS_DEBUG_NAMED("move_base","The global frame for move_base has changed, new frame: %s, new goal position x: %.2f, y: %.2f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);
        current_goal_pub_.publish(goal);

        //make sure to reset our timeouts and counters 确保重置我们的超时和计数器
        last_valid_control_ = ros::Time::now();
        last_valid_plan_ = ros::Time::now();
        last_oscillation_reset_ = ros::Time::now();
        planning_retries_ = 0;
      }

      //for timing that gives real time even in simulation
      ros::WallTime start = ros::WallTime::now();

      //the real work on pursuing a goal is done here





      bool done = executeCycle(goal, global_plan,message);
     //   bool done = executeCycle(goal, global_plan);
      //if we're done, then we'll return from execute

      if(isStartPid){


          if(done){
              if(message=="SUCCEEDED"||message=="ABORTED" ){
                  break;
              }

              return;
          }else{

              //check if execution of the goal has completed in some way
              ros::WallDuration t_diff = ros::WallTime::now() - start;
              ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec());

              r.sleep();

              if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
                  ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());

          }


      }else{


             if(done)
                return;

            //check if execution of the goal has completed in some way
            ros::WallDuration t_diff = ros::WallTime::now() - start;
            ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec());

            r.sleep();
            //make sure to sleep for the remainder of our cycle time

            if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
                ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());



      }



    }//todo while(n.ok())


//=start===========================================================================================================================
//  //sukai 精度 2022-08-31============================================================================================================================
///**
      if(message=="SUCCEEDED"||message=="ABORTED" ){
          goal_tolerance_pid=1;
          long int t_start = time(0);
          ros::Rate rate(10);
          while (ros::ok()) {

//              if(new_global_plan_){
//                  init();
//                  return;
//              }


              long int t1_now = time(0);
              long int t= t1_now-t_start;
              int  outTime =  time_out-int(t);


              try {

            

                  tf::StampedTransform transformtar;
                  listener.lookupTransform("/map", "/targetGoal", ros::Time(0), transformtar);

                  tf::Vector3 &origin = transformtar.getOrigin();
                  const tf::Quaternion &quaternion = transformtar.getRotation();
                  double tarroll, tarpitch, taryaw;
                  tf::Matrix3x3(quaternion).getRPY(tarroll, tarpitch, taryaw);

                  geometry_msgs::Pose target_pose;
                  target_pose.position.x=origin.x();
                  target_pose.position.y=origin.y();
                  target_pose.position.z=0;
                  target_pose.orientation.x = 0.0;
                  target_pose.orientation.y = 0.0;
                  target_pose.orientation.z = quaternion.getZ();
                  target_pose.orientation.w = quaternion.getW();



                  tf::StampedTransform transformcur;
                  listener.lookupTransform("/map", "/base_link", ros::Time(0), transformcur);
                  const tf::Quaternion &quaternioncur = transformcur.getRotation();
                  double rollcur, pitchcur, yawcur;
                  tf::Matrix3x3(quaternioncur).getRPY(rollcur, pitchcur, yawcur);

                  geometry_msgs::Pose cur_pose;
                  cur_pose.position.x=transformcur.getOrigin().x();
                  cur_pose.position.y=transformcur.getOrigin().y();
                  cur_pose.position.z=0;
                  cur_pose.orientation.x = 0.0;
                  cur_pose.orientation.y = 0.0;
                  cur_pose.orientation.z = quaternioncur.getZ();
                  cur_pose.orientation.w = quaternioncur.getW();



                  tf::StampedTransform transform;

                  listener.lookupTransform("/base_link", "/targetGoal", ros::Time(0), transform);
                  geometry_msgs::Pose targetGoalTObase_link;
                  targetGoalTObase_link.position.x=transform.getOrigin().x();
                  targetGoalTObase_link.position.y=transform.getOrigin().y();
                  targetGoalTObase_link.position.z=0;

                  double x = transform.getOrigin().x();
                  double y = transform.getOrigin().y();
                  double distance = sqrt(pow(x,2)+pow(y,2));
                  double angle = atan2(y,x);

                  const tf::Quaternion &quaternionrelative = transform.getRotation();
                  targetGoalTObase_link.orientation.x = 0.0;
                  targetGoalTObase_link.orientation.y = 0.0;
                  targetGoalTObase_link.orientation.z = quaternionrelative.getZ();
                  targetGoalTObase_link.orientation.w = quaternionrelative.getW();

                  double rollrelative, pitchrelative, yawrelative;
                  tf::Matrix3x3(quaternionrelative).getRPY(rollrelative, pitchrelative, yawrelative);
                  if(distance>distance_threshold){
                      geometry_msgs::Twist twist;
                     vel_pub_.publish(twist);


                      std_msgs::String datastr;
                      datastr.data= "ActionStatusError_distance_threshold";
                      xy_goal_pid_pub.publish(datastr);

                      as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
                      init();

                      return;
                  }


                  if(outTime<=0){

                      bool robot_stopped_ = isClosevariance(cur_pose,target_pose,(xy_goal_tolerance+tolerate_xy_goal_tolerance),(yaw_goal_tolerance+tolerate_yaw_goal_tolerance));
                      if(!robot_stopped_){

                          geometry_msgs::Twist twist;
                          vel_pub_.publish(twist);
                   
                          std_msgs::String datastr;
                          datastr.data= "ActionStatusError_time_out";
                          xy_goal_pid_pub.publish(datastr);
                          as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
                          init();

                      }else{
                          std_msgs::String datastr;
                          datastr.data= "ActionStatusFinished";
                          xy_goal_pid_pub.publish(datastr);
                          init();

                          geometry_msgs::Twist twist;
                          vel_pub_.publish(twist);

                          as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");

                      }

                      return;
                  }

                         
		
                           
                  if(goal_tolerance_pid==1){

                      bool robot_stopped_ =  isClosevariance_distanceAccuracy(cur_pose,target_pose ,xy_goal_tolerance);

                      if(!robot_stopped_){
                       // std::cout<<"==========angle:============"<<std::to_string(angle)<<std::endl;
                       // std::cout<<"==========yaw_goal_tolerance:============"<<std::to_string(yaw_goal_tolerance)<<std::endl;
                          if(abs(angle)<=yaw_goal_tolerance){


                              std_msgs::String datastr;
                              datastr.data= "ActionStatusRunning_turn";
                              xy_goal_pid_pub.publish(datastr);

                              geometry_msgs::Twist twist;
                              vel_pub_.publish(twist);
                              goal_tolerance_pid=2;
                                 distance_moveold=distance;


                          }else{

                              geometry_msgs::Twist twist;
                                      if(isfinite(angle)){
                              if(angle>M_PI){
                                  angle -= 2*M_PI;
                              } else if (angle<-M_PI){
                                  angle += 2*M_PI;
                              }

                                  double anglev=  angle/2;
                                  if(abs(anglev)>0.3){
                                      if(anglev>0){
                                            twist.angular.z =0.15;
                                       }else{
                                            twist.angular.z =-0.15;
                                          }
                                     
                                  }else if(abs(anglev)<0.020){
                                       if(anglev>0){
                                          twist.angular.z =0.02;
                                       }else{
                                          twist.angular.z =-0.02;
                                          }
                                      
                                  }else{
                                 
                                      twist.angular.z = anglev>0.1?(anglev/2):anglev ;
                                  }

                              }else{
                                  twist.angular.z =0.0;
                              }
                              vel_pub_.publish(twist);



                              std_msgs::String datastr;
                              datastr.data= "ActionStatusRunning_turn";
                              xy_goal_pid_pub.publish(datastr);


                          }

                      }else{


                          std_msgs::String datastr;
                          datastr.data= "ActionStatusRunning_turn";
                          xy_goal_pid_pub.publish(datastr);

                          geometry_msgs::Twist twist;
                          vel_pub_.publish(twist);
                          goal_tolerance_pid=3;
                          distance_moveold=distance;

                      }



                  } else if(goal_tolerance_pid==2){
                      double tarSpeed = sqrt(pow(x,2)+pow(y,2))/(time1*scan_time);

                      float pError = tarSpeed - curSpeed;

                      curSpeed += pError * kp;
                      float dError = pError - lastError;
                      curSpeed += dError * kd;
                      lastError = pError;

                      totalError += pError;
                      curSpeed+=totalError*ki;


                      if(distance<=xy_goal_tolerance){

                          std_msgs::String datastr;
                          datastr.data= "ActionStatusRunning_move";
                          xy_goal_pid_pub.publish(datastr);

                          geometry_msgs::Twist twist;
                          vel_pub_.publish(twist);
                          goal_tolerance_pid=3;
                          distance_moveold=distance;
            }else if(distance<distance_threshold&&(distance-distance_moveold)<0.05){


                          geometry_msgs::Twist twist;
                                
                          if(isfinite(curSpeed)){
                              if(curSpeed>0.3){
                                  twist.linear.x =0.1;

                              }else if(curSpeed<0.015) {
                                  twist.linear.x =0.019;
                              }else{
                                  twist.linear.x = curSpeed;
                              }
                          }else{
                              twist.linear.x =0.0;
                          }

                   

                         
                          if(isfinite(angle)){
                             if(angle>M_PI){
                                  angle -= 2*M_PI;
                              } else if (angle<-M_PI){
                                  angle += 2*M_PI;
                              }
                              double anglev=  angle/2;
                              if(abs(anglev)>0.3){
                             if(anglev>0){
                                           twist.angular.z =0.15;
                                       }else{
                                            twist.angular.z =-0.15;
                                          }
                                
                              }else if(abs(anglev)>0.010 &&abs(anglev)<0.020){
                                   if(anglev>0){
                                              twist.angular.z =0.02;
                                       }else{
                                             twist.angular.z =-0.02;
                                          }
                                
                              }else{
                                  
                                  twist.angular.z = 0.025 ;
                              }

                          }else{
                              twist.angular.z =0.0;
                          }

                        //  twist.angular.z =isfinite(angle)?angle/2:0.0;
                          vel_pub_.publish(twist);

                          std_msgs::String datastr;
                          datastr.data= "ActionStatusRunning_move";
                          xy_goal_pid_pub.publish(datastr);

            }else if((distance-distance_moveold)>0.05){//越走越远了，就需要重新调整转向了

                          std_msgs::String datastr;
                          datastr.data= "ActionStatusRunning_move";
                          xy_goal_pid_pub.publish(datastr);

                          geometry_msgs::Twist twist;
                          vel_pub_.publish(twist);
                          goal_tolerance_pid=1;
                         distance_moveold=0.0;
                      }


                  }else if(goal_tolerance_pid==3){

                      bool robot_stopped_angle = isClosevariance_angleAccuracy(cur_pose,target_pose,yaw_goal_tolerance);

                      if(robot_stopped_angle){

                          std_msgs::String datastr;
                          datastr.data= "ActionStatusRunning_attitude";
                          xy_goal_pid_pub.publish(datastr);

                          geometry_msgs::Twist twist;
                          vel_pub_.publish(twist);
                          goal_tolerance_pid=4;
                          distance_moveold=0.0;

                      }else{
                          if((distance-distance_moveold)>0.05){

                              std_msgs::String datastr;
                              datastr.data= "ActionStatusRunning_attitude";
                              xy_goal_pid_pub.publish(datastr);

                              geometry_msgs::Twist twist;
                              vel_pub_.publish(twist);
                              goal_tolerance_pid=1;
                              distance_moveold=0.0;
                              continue;
                          }

                        
                          geometry_msgs::Twist twist;
                          
                             if(isfinite(yawrelative)){
                              if(yawrelative>M_PI){
                                  yawrelative -= 2*M_PI;
                              } else if (angle<-M_PI){
                                  yawrelative += 2*M_PI;
                              }
                              double anglev=  yawrelative/2;
                           
                              if(abs(anglev)>0.3){
                                    if(anglev>0){
                                            twist.angular.z =0.15;
                                       }else{
                                            twist.angular.z =-0.15;
                                          }
                                  
                              }else if(abs(anglev)<0.020){
                                      if(anglev>0){
                                         twist.angular.z =0.02;
                                       }else{
                                         twist.angular.z =-0.02;
                                          }
                                 
                              }else{
                                 
                                  twist.angular.z = anglev>0.1?(anglev/2):anglev ;
                              }

                          }else{
                              twist.angular.z =0.0;
                          }



                          vel_pub_.publish(twist);

                          std_msgs::String datastr;
                          datastr.data= "ActionStatusRunning_attitude";
                          xy_goal_pid_pub.publish(datastr);



                      }

                  }else if(goal_tolerance_pid==4) {
                      bool robot_stopped_ = isClosevariance(cur_pose,target_pose,(xy_goal_tolerance+tolerate_xy_goal_tolerance),(yaw_goal_tolerance+tolerate_yaw_goal_tolerance)); 
                      if(!robot_stopped_){

                          std_msgs::String datastr;
                          datastr.data= "ActionStatusRunning_again";
                          xy_goal_pid_pub.publish(datastr);

                          geometry_msgs::Twist twist;
                          vel_pub_.publish(twist);
                          goal_tolerance_pid=1;
                             distance_moveold=0.0;
                      }else{

                          std_msgs::String datastr;
                          datastr.data= "ActionStatusFinished";
                          xy_goal_pid_pub.publish(datastr);
                           init();
                          geometry_msgs::Twist twist;
                          vel_pub_.publish(twist);

                          as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");

                          return;

                      }

                  }

                  distanceold=distance;

              } catch (std::exception e) {
                  rate.sleep();
                 // return;
              }
              rate.sleep();
          }


      }

//*/

//============================================================================================================================
//=end===========================================================================================================================






    //wake up the planner thread so that it can exit cleanly 唤醒planner线程，使其可以干净地退出
    lock.lock();
    runPlanner_ = true;
    planner_cond_.notify_one();
    lock.unlock();

    //if the node is killed then we'll abort and return 如果节点被杀死，那么我们将中止并返回
   //失败
    as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed");
    return;
  }

  double MoveBase::distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2)
  {
    return hypot(p1.pose.position.x - p2.pose.position.x, p1.pose.position.y - p2.pose.position.y);
  }


    bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan,std::string &message){

  //   bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan){
    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
    //we need to be able to publish velocity commands 我们需要能够发布速度命令
    geometry_msgs::Twist cmd_vel;
   //1.获取机器人当前位置
    //update feedback to correspond to our curent position 更新反馈以符合我们当前的立场
//通过MoveBase的成员函数getRobotPose获取全局代价地图上的机器人位姿

    geometry_msgs::PoseStamped global_pose;
    getRobotPose(global_pose, planner_costmap_ros_);
//把当前位姿转换为geometry_msgs::PoseStamped格式，存储在current_position中

    const geometry_msgs::PoseStamped& current_position = global_pose;
//发布反馈消息到Action的客户端，feedback（结构体）指的是从服务端周期反馈回客户端的信息，把当前位姿反馈给客户端

    //push the feedback out 将反馈推出
    move_base_msgs::MoveBaseFeedback feedback;
    feedback.base_position = current_position;
    as_->publishFeedback(feedback);
 //2.重置震荡标志位
    //check to see if we've moved far enough to reset our oscillation timeout 检查我们是否移动了足够的距离来重置振荡超时
    if(distance(current_position, oscillation_pose_) >= oscillation_distance_)
    {
      last_oscillation_reset_ = ros::Time::now();
      oscillation_pose_ = current_position;
  //如果我们上一次的恢复行为是由振荡引起，我们就重新设置恢复行为的索引

      //if our last recovery was caused by oscillation, we want to reset the recovery index 如果我们上次的恢复是由振荡引起的，我们需要重置恢复指数
      if(recovery_trigger_ == OSCILLATION_R)
        recovery_index_ = 0;
    }
  //3.地图数据超时，即观测传感器数据不够新,停止机器人,返回false
    //check that the observation buffers for the costmap are current, we don't want to drive blind
//检查costmap的观察缓冲区是否当前，我们不想盲目驾驶
//controller_costmap_ros_是MoveBase的局部代价地图，通过其接口isCurrent()，我们可以查看代价地图近期是否得到了更新。
//如果长时间没有更新，意味着机器人失去了周围的感知能力， 此时移动机器人是危险的，所以我们通过publishZeroVelocity控制机器人停下来并退出业务逻辑

    if(!controller_costmap_ros_->isCurrent()){
      ROS_WARN("[%s]:Sensor data is out of date, we're not going to allow commanding of the base for safety",ros::this_node::getName().c_str());
      publishZeroVelocity();
      return false;
    }
     //4. 如果获取新的全局路径,则将它传输给控制器，完成latest_plan_到controller_plan_的转换
//通过标志位判定全局规划是否得出新的路线，然后通过指针交换，将latest_plan_（最新的全局规划结果）的值传递给controller_plan_即局部规划使用，
//然后将上一次的局部规划路线传递给latest_plan
    //if we have a new plan then grab it and give it to the controller 如果我们有一个新的计划，那么抓住它并把它交给控制器
    if(new_global_plan_){
      //make sure to set the new plan flag to false 确保将新计划标志设置为false
      new_global_plan_ = false;

      ROS_DEBUG_NAMED("move_base","Got a new plan...swap pointers");

      //do a pointer swap under mutex 在互斥下进行指针交换
      std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_;

      boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
      controller_plan_ = latest_plan_;
      latest_plan_ = temp_plan;
      lock.unlock();
      ROS_DEBUG_NAMED("move_base","pointers swapped!");
//5. 给控制器设置全局路径
  //在实例tc_上调用局部规划器BaseLocalPlanner的类函数setPlan()， 把全局规划的结果传递给局部规划器，如果传递失败，退出并返回

      if(!tc_->setPlan(*controller_plan_)){
        //ABORT and SHUTDOWN COSTMAPS 中止和关闭成本图
        ROS_ERROR("Failed to pass global plan to the controller, aborting.");
        resetState();

        //disable the planner thread
        lock.lock();
        runPlanner_ = false;
        lock.unlock();
//失败 //6.设置动作中断,返回true 无法将全局计划传递给控制器
        as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");
        return true;
      }
//如果全局路径有效，则不需要recovery
  //如果我们找到有效的局部规划路线，且恢复行为是“全局规划失败”，重置恢复行为索引

      //make sure to reset recovery_index_ since we were able to find a valid plan
      if(recovery_trigger_ == PLANNING_R)
        recovery_index_ = 0;
    }
//对MoveBase状态进行判断，由于局部规划在全局规划结束后才调用，所以有以下几种结果：
//① PLANNING：全局规划还没完成，还没得到一个全局路线，那么唤醒一个全局规划线程去制定全局路线
//② CONTROLLING：全局规划成功，得到全局路线，这里进行真正的局部规划，实际控制机器人沿着轨迹运动的状态
//③ CLEARING：全局规划失败，进入恢复行为，它调用用户定义的恢复逻辑尝试清除状态空间

    //the move_base state machine, handles the control logic for navigation
    switch(state_){
      //if we are in a planning state, then we'll attempt to make a plan
      case PLANNING:
        {
          boost::recursive_mutex::scoped_lock lock(planner_mutex_);
          runPlanner_ = true;
          planner_cond_.notify_one();//还在PLANNING中则唤醒规划线程让它干活
        }
        ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state.");
        break;
 //7.机器人控制状态,尝试获取一个有效的速度命令
      //if we're controlling, we'll attempt to find valid velocity commands
      case CONTROLLING:
        ROS_DEBUG_NAMED("move_base","In controlling state.");
        //8.如果到达目标点,重置状态,设置动作成功,返回true
        //check to see if we've reached our goal
        if(tc_->isGoalReached()){
          ROS_DEBUG_NAMED("move_base","Goal reached!");
          resetState();

          //disable the planner thread
          boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
          runPlanner_ = false;
          lock.unlock();



            message="SUCCEEDED";

            if(!isStartPid){
                 as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
            }



          return true;
        }
//9. 如果超过震荡时间,停止机器人,设置清障标志位
        //返回去继续看，如果超过震荡时间，停止机器人，设置清障标志位：震荡说明遇到了障碍，机器人在哪里晃

        //check for an oscillation condition
        if(oscillation_timeout_ > 0.0 &&
            last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
        {
          publishZeroVelocity();
          state_ = CLEARING;
          recovery_trigger_ = OSCILLATION_R;
        }

        {
         boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_ros_->getCostmap()->getMutex()));
//10. 获取有效速度,如果获取成功，直接发布到cmd_vel
 ///局部规划器实例tc_被传入了全局规划后，调用computeVelocityCommands函数计算速度存储在cmd_vel中
     //其中变量 tc_ 是teb_local_planner_ros插件类的实例,是一个local_planner对象的共享指针(boost::shared_ptr<nav_core::BaseLocalPlanner>)，
     //在move_base类的构造函数中创建和初始化,调用local_planner的函数主要是tc_->computeVelocityCommands(cmd_vel)，就是这个函数开始调用teb_local_planner算法
 //获取有效速度,如果获取成功，直接发布到cmd_vel：应该是局部速度，

        if(tc_->computeVelocityCommands(cmd_vel)){
          ROS_DEBUG_NAMED( "move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",
                           cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z );

                //若成功计算速度，上一次有效局部控制的时间设为当前
                last_valid_control_ = ros::Time::now();
                //make sure that we send the velocity command to the base
                //向底盘发送速度控制消息，一个循环只发一次速度命令
                vel_pub_.publish(cmd_vel);
                if(recovery_trigger_ == CONTROLLING_R)
                    recovery_index_ = 0;


        }
        else {
  //11.如果没有获取到有效速度，该判断有没有超时
          ROS_DEBUG_NAMED("move_base", "The local planner could not find a valid plan.");
          ros::Time attempt_end = last_valid_control_ + ros::Duration(controller_patience_);
//12.判断超过尝试时间,如果超时，停止机器人,进入清障模式

      //若局部规划用时超过限制
          //check if we've tried to find a valid control for longer than our time limit
          if(ros::Time::now() > attempt_end){
              //todo sukai 2022-12-26
              if(ispubobstacle){//发送障碍物消息，并阻止小车前进
                  publishZeroVelocity();
                  std_msgs::String data;
                  data.data="showobstacle";
                  pubobstacle_pub_.publish(data);

              }else{
                  //we'll move into our obstacle clearing mode
                  publishZeroVelocity();
                  state_ = CLEARING;//规划失败了
                  recovery_trigger_ = CONTROLLING_R; //若局部规划用时超过限制
              }


          }
          else{//若局部规划用时没超过限制



            //todo sukai 2022-12-26
              if(ispubobstacle){//发送障碍物消息，并阻止小车前进

                  publishZeroVelocity();
                  std_msgs::String data;
                  data.data="showobstacle";
                  pubobstacle_pub_.publish(data);

              }else{
                  //如果不超时，让全局再规划一个路径
                  //otherwise, if we can't find a valid control, we'll go back to planning
                  last_valid_plan_ = ros::Time::now();
                  planning_retries_ = 0;
                  state_ = PLANNING;
                  publishZeroVelocity();

                  //enable the planner thread in case it isn't running on a clock
                  boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
                  runPlanner_ = true;
                  planner_cond_.notify_one();
                  lock.unlock();
              }

          }
        }
        }

        break;
 //13. 规划或者控制失败，恢复或者进入到清障模式 //③ CLEARING：全局规划失败，进入恢复行为，它调用用户定义的恢复逻辑尝试清除状态空间
      //we'll try to clear out space with any user-provided recovery behaviors
      case CLEARING:
        ROS_DEBUG_NAMED("move_base","In clearing/recovery state");
        //we'll invoke whatever recovery behavior we're currently on if they're enabled
        if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){
          ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_+1, recovery_behaviors_.size());

          move_base_msgs::RecoveryStatus msg;
          msg.pose_stamped = current_position;
          msg.current_recovery_number = recovery_index_;
          msg.total_number_of_recoveries = recovery_behaviors_.size();
          msg.recovery_behavior_name =  recovery_behavior_names_[recovery_index_];

          recovery_status_pub_.publish(msg);

          recovery_behaviors_[recovery_index_]->runBehavior();

          //we at least want to give the robot some time to stop oscillating after executing the behavior
          last_oscillation_reset_ = ros::Time::now();

          //we'll check if the recovery behavior actually worked
          ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state");
          last_valid_plan_ = ros::Time::now();
          planning_retries_ = 0;
          state_ = PLANNING;

          //update the index of the next recovery behavior that we'll try
          recovery_index_++;
        }
        else{ //若恢复行为无效
          //所有恢复行为都已失败，锁定规划器并将其禁用
          ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it.");
          //disable the planner thread
          boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
          runPlanner_ = false;
          lock.unlock();
              //在此之后，某些东西应该中止
          ROS_DEBUG_NAMED("move_base_recovery","Something should abort after this.");
                  //反馈失败的具体信息
               if(recovery_trigger_ == CONTROLLING_R){//todo 未到达指定位置
              //正在中止，因为找不到有效控件。即使在执行所有恢复行为之后
            ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors");
                   message="ABORTED";
                   if(!isStartPid) {
                         as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
                   }

          }
          else if(recovery_trigger_ == PLANNING_R){
            ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors");
            //未到达，失败 正在中止，因为找不到有效的计划。即使在执行所有恢复行为之后
            as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
          }
          else if(recovery_trigger_ == OSCILLATION_R){
            ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");
            //未到达，失败 正在中止，因为机器人似乎在反复摆动。即使在执行所有恢复行为之后
            as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
          }
          resetState();
          return true;
        }
        break;
      default:
        ROS_ERROR("This case should never be reached, something is wrong, aborting");
        resetState();
        //disable the planner thread
        boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
        runPlanner_ = false;
        lock.unlock();
//未到达，失败
        as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it.");
        return true;
    }

    //we aren't done yet
    return false;
  }
 //recovery是指恢复的规划器，其跟全局规划器和局部规划器是同一个等级的。
 //不同的是，它是在机器人在局部代价地图或者全局代价地图中找不到路时才会被调用，比如rotate_recovery让机器人原地360°旋转，clear_costmap_recovery将代价地图恢复到静态地图的样子。
  bool MoveBase::loadRecoveryBehaviors(ros::NodeHandle node){
    XmlRpc::XmlRpcValue behavior_list;
    if(node.getParam("recovery_behaviors", behavior_list)){
      if(behavior_list.getType() == XmlRpc::XmlRpcValue::TypeArray){
        for(int i = 0; i < behavior_list.size(); ++i){
          if(behavior_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct){
            if(behavior_list[i].hasMember("name") && behavior_list[i].hasMember("type")){
              //check for recovery behaviors with the same name
              for(int j = i + 1; j < behavior_list.size(); j++){
                if(behavior_list[j].getType() == XmlRpc::XmlRpcValue::TypeStruct){
                  if(behavior_list[j].hasMember("name") && behavior_list[j].hasMember("type")){
                    std::string name_i = behavior_list[i]["name"];
                    std::string name_j = behavior_list[j]["name"];
                    if(name_i == name_j){
                      ROS_ERROR("A recovery behavior with the name %s already exists, this is not allowed. Using the default recovery behaviors instead.",
                          name_i.c_str());
                      return false;
                    }
                  }
                }
              }
            }
            else{
              ROS_ERROR("Recovery behaviors must have a name and a type and this does not. Using the default recovery behaviors instead.");
              return false;
            }
          }
          else{
            ROS_ERROR("Recovery behaviors must be specified as maps, but they are XmlRpcType %d. We'll use the default recovery behaviors instead.",
                behavior_list[i].getType());
            return false;
          }
        }

        //if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors
        for(int i = 0; i < behavior_list.size(); ++i){
          try{
            //check if a non fully qualified name has potentially been passed in
            if(!recovery_loader_.isClassAvailable(behavior_list[i]["type"])){
              std::vector<std::string> classes = recovery_loader_.getDeclaredClasses();
              for(unsigned int i = 0; i < classes.size(); ++i){
                if(behavior_list[i]["type"] == recovery_loader_.getName(classes[i])){
                  //if we've found a match... we'll get the fully qualified name and break out of the loop
                  ROS_WARN("Recovery behavior specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.",
                      std::string(behavior_list[i]["type"]).c_str(), classes[i].c_str());
                  behavior_list[i]["type"] = classes[i];
                  break;
                }
              }
            }

            boost::shared_ptr<nav_core::RecoveryBehavior> behavior(recovery_loader_.createInstance(behavior_list[i]["type"]));

            //shouldn't be possible, but it won't hurt to check
            if(behavior.get() == NULL){
              ROS_ERROR("The ClassLoader returned a null pointer without throwing an exception. This should not happen");
              return false;
            }

            //initialize the recovery behavior with its name
            behavior->initialize(behavior_list[i]["name"], &tf_, planner_costmap_ros_, controller_costmap_ros_);
            recovery_behavior_names_.push_back(behavior_list[i]["name"]);
            recovery_behaviors_.push_back(behavior);
          }
          catch(pluginlib::PluginlibException& ex){
            ROS_ERROR("Failed to load a plugin. Using default recovery behaviors. Error: %s", ex.what());
            return false;
          }
        }
      }
      else{
        ROS_ERROR("The recovery behavior specification must be a list, but is of XmlRpcType %d. We'll use the default recovery behaviors instead.",
            behavior_list.getType());
        return false;
      }
    }
    else{
      //if no recovery_behaviors are specified, we'll just load the defaults
      return false;
    }

    //if we've made it here... we've constructed a recovery behavior list successfully
    return true;
  }

  //we'll load our default recovery behaviors here
  void MoveBase::loadDefaultRecoveryBehaviors(){
    recovery_behaviors_.clear();
    try{
      //we need to set some parameters based on what's been passed in to us to maintain backwards compatibility
      ros::NodeHandle n("~");
      n.setParam("conservative_reset/reset_distance", conservative_reset_dist_);
      n.setParam("aggressive_reset/reset_distance", circumscribed_radius_ * 4);
         //1、加载recovery behavior清理costmap

      //first, we'll load a recovery behavior to clear the costmap
      boost::shared_ptr<nav_core::RecoveryBehavior> cons_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
      cons_clear->initialize("conservative_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
      recovery_behavior_names_.push_back("conservative_reset");
      recovery_behaviors_.push_back(cons_clear);
  //2、加载recovery behavior 原地旋转

      //next, we'll load a recovery behavior to rotate in place
      boost::shared_ptr<nav_core::RecoveryBehavior> rotate(recovery_loader_.createInstance("rotate_recovery/RotateRecovery"));
      if(clearing_rotation_allowed_){
        rotate->initialize("rotate_recovery", &tf_, planner_costmap_ros_, controller_costmap_ros_);
        recovery_behavior_names_.push_back("rotate_recovery");
        recovery_behaviors_.push_back(rotate);
      }
  //3、加载 recovery behavior 重置 costmap
      //next, we'll load a recovery behavior that will do an aggressive reset of the costmap
      boost::shared_ptr<nav_core::RecoveryBehavior> ags_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
      ags_clear->initialize("aggressive_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
      recovery_behavior_names_.push_back("aggressive_reset");
      recovery_behaviors_.push_back(ags_clear);
  //4、再一次旋转
      //we'll rotate in-place one more time
      if(clearing_rotation_allowed_){
        recovery_behaviors_.push_back(rotate);
        recovery_behavior_names_.push_back("rotate_recovery");
      }
    }
    catch(pluginlib::PluginlibException& ex){
      ROS_FATAL("Failed to load a plugin. This should not happen on default recovery behaviors. Error: %s", ex.what());
    }

    return;
  }

  void MoveBase::resetState(){
    // Disable the planner thread
    boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
    runPlanner_ = false;
    lock.unlock();

    // Reset statemachine
    state_ = PLANNING;
    recovery_index_ = 0;
    recovery_trigger_ = PLANNING_R;
    publishZeroVelocity();

    //if we shutdown our costmaps when we're deactivated... we'll do that now
    if(shutdown_costmaps_){
      ROS_DEBUG_NAMED("move_base","Stopping costmaps");
      planner_costmap_ros_->stop();
      controller_costmap_ros_->stop();
    }
  }

  bool MoveBase::getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap)
  {
    tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
    geometry_msgs::PoseStamped robot_pose;
    tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
    robot_pose.header.frame_id = robot_base_frame_;
    robot_pose.header.stamp = ros::Time(); // latest available
    ros::Time current_time = ros::Time::now();  // save time for checking tf delay later
// 转换到统一的全局坐标
    // get robot pose on the given costmap frame
    try
    {
      tf_.transform(robot_pose, global_pose, costmap->getGlobalFrameID());
    }
    catch (tf2::LookupException& ex)
    {
      ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
      return false;
    }
    catch (tf2::ConnectivityException& ex)
    {
      ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
      return false;
    }
    catch (tf2::ExtrapolationException& ex)
    {
      ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
      return false;
    }
// 全局坐标时间戳是否在costmap要求下
    // check if global_pose time stamp is within costmap transform tolerance
    if (current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance())
    {
      ROS_WARN_THROTTLE(1.0, "Transform timeout for %s. " \
                        "Current time: %.4f, pose stamp: %.4f, tolerance: %.4f", costmap->getName().c_str(),
                        current_time.toSec(), global_pose.header.stamp.toSec(), costmap->getTransformTolerance());
      return false;
    }

    return true;
  }
};
